#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wcast-qual"
#pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant"
#pragma GCC diagnostic ignored "-Wenum-compare"
#pragma GCC diagnostic ignored "-Wdouble-promotion"
#pragma GCC diagnostic ignored "-Wunknown-pragmas"
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#include <buola/cv/ccloudview.h>
#include <buola/app/ceventloop.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <buola/mat/transformations.h>
#include <buola/cv/coccupancyoctree.h>
#include <buola/mat/cgaussianprocess.h>
#pragma GCC diagnostic pop

using namespace buola;

int main(int pNArg,char **pArgs)
{
    pcl::PointCloud<pcl::PointXYZRGBA> lPCL;

    start_timer();
    pcl::io::loadPCDFile((io::get_home()/"tabletop.pcd").Path(),lPCL);
    end_timer();

    buola_init(pNArg,pArgs);

    msg_info() << lPCL.width << "x" << lPCL.height << "\n";
    msg_info() << lPCL(0,0) << "\n";

//    return CPoint_d(pP.x()/pP.z()*mMatrix[0]+mMatrix[2],pP.y()/pP.z()*mMatrix[4]+mMatrix[5]);
//    px=pP.x()/pP.z()*pF+320;
//    pF=(px-320)*pP.z()/pP.x()

    for(int i=0;i<640;i++)
    {
        for(int j=0;j<480;j++)
        {
            if(!is_nan(lPCL(i,j).z))
            {
                double lFX=(i-320.0f)*lPCL(i,j).z/lPCL(i,j).x;
                double lFY=(j-240.0f)*lPCL(i,j).z/lPCL(i,j).y;
                
                msg_info() << i << " " << j << " " << lFX << " " << lFY << "\n";
            }
        }
    }
    

    return buola_finish();
}
